//------------------------------------------------------------------------------
//  <copyright file="ImageOperations2DInterop.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#include "ImageFrame.h"

using namespace System;
using namespace System::IO;
using namespace Microsoft::Robotics::Vision;

/// <summary>
/// Initializes a new instance of the <see cref="ImageFrameNormal"/> class.
/// from a depth image
/// </summary>
/// <param name="depthImage">Depth image</param>
/// <param name="fx">Focal length x in pixels (must be positive)</param>
/// <param name="fy">Focal length y in pixels (must be positive)</param>
ImageFrameNormal::ImageFrameNormal(ImageFrameDepth^ depthImage, double fx, double fy) : ImageFrame(depthImage->Width, depthImage->Height)
{
	if (fx <= 0.0 || fy <= 0.0)
	{
		throw gcnew ArgumentException(String::Format("Expect positive focal length found fx {0}  fy {1}", fx, fy));
	}

	this->fx = fx;
	this->fy = fy;
	this->Init(depthImage);
}

/// <summary>
/// Heavy lifting to initialize frame normal
/// </summary>
/// <param name="depthImage">Depth image</param>
void ImageFrameNormal::Init(ImageFrameDepth^ depthImage)
{
	// All pixels except the border ones are consistent
	for(int col = 2 ; col < this->Width - 1; ++col)
	{
		for(int row = 2 ; row < this->Height - 1; ++row)
		{
			this->default[col, row] = this->ComputeNormals(depthImage, col, row);
		}
	}

	// Special case the border pixels
	int rowStart = 0;
	int rowEnd = this->Height - 1;
	for(int col = 1 ; col < this->Width - 1; ++col)
	{
		this->default[col, rowStart] = this->ComputeStartRowNormals(depthImage, col);
		this->default[col, rowEnd] = this->ComputeEndRowNormals(depthImage, col);
	}

	int colStart = 0;
	int colEnd = this->Width - 1;
	for(int row = 1 ; row < this->Height - 1; ++row)
	{
		this->default[colStart, row] = this->ComputeStartColNormals(depthImage, row);
		this->default[colEnd, row] = this->ComputeEndColNormals(depthImage, row);
	}
}

/// <summary>
/// Compute normal at a row column location
/// </summary>
/// <param name="depthImage">Depth image</param>
/// <param name="col">Column location</param>
/// <param name="row">Row location</param>
PixNormal ImageFrameNormal::ComputeNormals(ImageFrameDepth^ depthImage, int col, int row)
{
	float delX = (float)((depthImage[col + 1, row] - depthImage[col - 1, row]) * depthImage[col, row] / this->FocalX) ;
	float delY = (float)((depthImage[col, row + 1] - depthImage[col, row - 1]) * depthImage[col, row] / this->FocalY);
	float delZ = 2.0;
	float sum = sqrt(delX * delX + delY * delY + delZ * delZ);

	PixNormal pixN = (PixNormal)(gcnew PixNormal((SByte)((delX / sum) * SByte::MaxValue), (SByte)((delY / sum) * SByte::MaxValue), (SByte)((1 / sum) * SByte::MaxValue)));
	return pixN;
}

/// <summary>
/// Special case to compute normal in the first column
/// </summary>
/// <param name="depthImage">Depth image</param>
/// <param name="row">Row location</param>
PixNormal ImageFrameNormal::ComputeStartColNormals(ImageFrameDepth^ depthImage, int row)
{
	int col = 0;
	float delX = (float)((depthImage[col + 1, row] - depthImage[col, row]) * depthImage[col, row] / this->FocalX) * 2.0F;
	float delY = (float)((depthImage[col, row + 1] - depthImage[col, row - 1]) * depthImage[col, row] / this->FocalY);
	float delZ = 2.0;
	float sum = sqrt(delX * delX + delY * delY + delZ * delZ);

	PixNormal pixN = (PixNormal)(gcnew PixNormal((SByte)(delX / sum * SByte::MaxValue), (SByte)(delY / sum * SByte::MaxValue), (SByte)(1 / sum * SByte::MaxValue)));
	return pixN;
}

/// <summary>
/// Special case to compute normal in the last column
/// </summary>
/// <param name="depthImage">Depth image</param>
/// <param name="row">Row location</param>
PixNormal ImageFrameNormal::ComputeEndColNormals(ImageFrameDepth^ depthImage, int row)
{
	int col = this->Width - 1;
	float delX = (float)((depthImage[col, row] - depthImage[col - 1, row]) * depthImage[col, row] / this->FocalX) * 2.0F;
	float delY = (float)((depthImage[col, row + 1] - depthImage[col, row - 1]) * depthImage[col, row] / this->FocalY);
	float delZ = 2.0;
	float sum = sqrt(delX * delX + delY * delY + delZ * delZ);

	PixNormal pixN = (PixNormal)(gcnew PixNormal((SByte)(delX / sum * SByte::MaxValue), (SByte)(delY / sum * SByte::MaxValue), (SByte)(1 / sum * SByte::MaxValue)));
	return pixN;
}

/// <summary>
/// Special case to compute normal in the first row
/// </summary>
/// <param name="depthImage">Depth image</param>
/// <param name="col">col location</param>
PixNormal ImageFrameNormal::ComputeStartRowNormals(ImageFrameDepth^ depthImage, int col)
{
	int row = 0;
	float delX = (float)((depthImage[col + 1, row] - depthImage[col - 1, row]) * depthImage[col, row] / this->FocalX) ;
	float delY = (float)((depthImage[col, row + 1] - depthImage[col, row]) * depthImage[col, row] / this->FocalY) * 2.0F;
	float delZ = 2.0;
	float sum = sqrt(delX * delX + delY * delY + delZ * delZ);

	PixNormal pixN = (PixNormal)(gcnew PixNormal((SByte)(delX / sum * SByte::MaxValue), (SByte)(delY / sum * SByte::MaxValue), (SByte)(delZ / sum * SByte::MaxValue)));
	return pixN;
}

/// <summary>
/// Special case to compute normal in the last row
/// </summary>
/// <param name="depthImage">Depth image</param>
/// <param name="col">col location</param>
PixNormal ImageFrameNormal::ComputeEndRowNormals(ImageFrameDepth^ depthImage, int col)
{
	int row = this->Height - 1;
	float delX = (float)((depthImage[col + 1, row] - depthImage[col - 1, row]) * depthImage[col, row] / this->FocalX) ;
	float delY = (float)((depthImage[col, row] - depthImage[col, row - 1]) * depthImage[col, row] / this->FocalY) * 2.0F;
	float delZ = 2.0;
	float sum = sqrt(delX * delX + delY * delY + delZ * delZ);

	PixNormal pixN = (PixNormal)(gcnew PixNormal((SByte)(delX / sum * SByte::MaxValue), (SByte)(delY / sum * SByte::MaxValue), (SByte)(1 / sum * SByte::MaxValue)));
	return pixN;
}
